%% 滑模控制1
% 惟一需要手算的是控制变量u
% 看看熊制导的PPT51-53页就可以了
clear all;
clc;

%% 参数初始化
% dX = AX+Bu
% s = CX
% ds = -epsilon*sgn(s)-k*s
matA = [0, 1; 0, -25];
matB = [0; 133];
matC = [15, 1];
epsilon = 5;
gainK = 10;

X = [0.5; 0.5];         %初始状态

dt = 0.001;             %仿真步长(s)
T = 1;                  %仿真周期(s)

%% 画图记录量
time = 0:dt:T;
xLog = zeros(2, T/dt+1);
sLog = zeros(1, T/dt+1);
uLog = zeros(1, T/dt+1);

%% 滑模控制
for i = 1:1:T/dt+1
    % 手动推导得到的滑模控制量
    s = matC*X;
    u = (-matC*matA*X -epsilon*sign(s) -gainK*s) / (matC*matB);
    % 状态方程
    dX = matA*X + matB*u;
    % 状态线性递推
    X = X + dX*dt;
    % 记录
    xLog(:, i) = X;
    sLog(i) = s;
    uLog(i) = u;
end

%% 画图
figure;
plot(time, xLog, 'linewidth', 1.5);
hold on;
plot(time, sLog, 'linewidth', 1.5);
grid on;
title('Sliding Mode Control');
xlabel('time(s)');
ylabel('variables');
legend('x1', 'x2', 's');

figure;
plot(xLog(1, :), xLog(2, :), 'linewidth', 1.5);
hold on;
plot(xLog(1, :), -matC(1)*xLog(1, :)/matC(2), 'linewidth', 1.5);
grid on;
title('Sliding Mode Control');
xlabel('x1');
ylabel('x2');
legend('相轨迹', '线性滑膜面');
axis([0, 0.6, -7, 0]);

figure;
plot(time, uLog, 'linewidth', 1.5);
grid on;
title('Sliding Mode Control');
xlabel('time(s)');
ylabel('u');
legend('u');
